﻿// <copyright file="Polyhedron.cs" company="Community Combat Simulator">
//    Community Combat Simulator: a free combat flight simulator.
//    Copyright (C) 2008 Aaron Schinkowitch
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see http://www.gnu.org/licenses/.
// </copyright>
namespace CCSim.Vecmath
{
    using System;

    /// <summary>
    /// Description of Polyhedron.
    /// </summary>
    public class Polyhedron
    {
        /// <summary>
        /// Number of vertices that defines a face.
        /// </summary>
        private const int VerticesPerFace = 3;
        
        /// <summary>
        /// The faces that make up the polyhedron.
        /// </summary>
        private Face[] faces;
        
        /// <summary>
        /// The vertices that make up the polyhedron in an untransformed state.
        /// </summary>
        private Vector[] rawVertices;
        
        /// <summary>
        /// The vertices that make up the polyhedron in a transformed state.
        /// </summary>
        private Vector[] transformedVertices;
        
        /// <summary>
        /// The normals of the faces in an untransformed state.
        /// </summary>
        private Vector[] rawNormals;
        
        /// <summary>
        /// The normals of the faces in a transformed.
        /// </summary>
        private Vector[] transformedNormals;
        
        /// <summary>
        /// Initializes a new instance of the Polyhedron class from vertices and vertex indices.
        /// </summary>
        /// <param name="vertices">The vertices that make up the polyhedron.</param>
        /// <param name="indexes">The indices that define the faces of the polyhedron. Each set of
        /// three indices in the array defines one face of the polyhedron.</param>
        public Polyhedron(Vector[] vertices, int[] indexes)
        {
            if (vertices == null) 
            {
                throw new ArgumentNullException("vertices");
            }
            
            if (indexes == null)
            {
                throw new ArgumentNullException("indexes");
            }
            
            if (indexes.Length % VerticesPerFace != 0)
            {
                throw new ArgumentException(
                        "Indices must be evenly divisible by " + VerticesPerFace
                            + ". Was length: " + indexes.Length);
            }
            
            int faceCount = indexes.Length / VerticesPerFace;
            
            // And of the corresponding tranformed vectors
            this.TransformedVertices = new Vector[vertices.Length];
            this.TransformedNormals = new Vector[faceCount];
            
            Array.Copy(vertices, this.TransformedVertices, vertices.Length);
            
            Face[] faces = new Face[faceCount];
            
            int indicesIndex = 0;
            for (int faceIndex = 0; faceIndex < faces.Length; faceIndex++)
            {
                Face face = new Face(
                    vertices[indexes[indicesIndex++]], 
                    vertices[indexes[indicesIndex++]], 
                    vertices[indexes[indicesIndex++]]);
                faces[faceIndex] = face;
                
                // Add the face's normal to the transformed list
                this.TransformedNormals[faceIndex] = face.Normal;
            }
            
            this.Faces = faces;
            
            // We'll hold a copy of the original vertices and the face normals
            this.RawVertices = new Vector[this.TransformedVertices.Length];
            
            for (int i = 0; i < this.TransformedVertices.Length; i++) 
            {
                this.RawVertices[i] = new Vector(this.TransformedVertices[i]);
            }
            
            this.RawNormals = new Vector[this.TransformedNormals.Length];
            
            for (int i = 0; i < this.TransformedNormals.Length; i++)
            {
                this.RawNormals[i] = new Vector(this.TransformedNormals[i]);
            }
        }
        
        /// <summary>
        /// Gets or sets the faces that make up the polyhedron.
        /// </summary>
        internal Face[] Faces 
        {
            get { return this.faces; }
            set { this.faces = value; }
        }
        
        /// <summary>
        /// Gets or sets the vertices that make up the polyhedron in an untransformed state.
        /// </summary>
        internal Vector[] RawVertices 
        {
            get { return this.rawVertices; }
            set { this.rawVertices = value; }
        }
        
        /// <summary>
        /// Gets or sets the vertices that make up the polyhedron in a transformed state.
        /// </summary>
        internal Vector[] TransformedVertices 
        {
            get { return this.transformedVertices; }
            set { this.transformedVertices = value; }
        }
        
        /// <summary>
        /// Gets or sets the normals of the faces of the polyhedron in a non-transformed state.
        /// </summary>
        internal Vector[] RawNormals 
        {
            get { return this.rawNormals; }
            set { this.rawNormals = value; }
        }
        
        /// <summary>
        /// Gets or sets the normals of the faces of the polyhedron in a transformed state.
        /// </summary>
        internal Vector[] TransformedNormals 
        {
            get { return this.transformedNormals; }
            set { this.transformedNormals = value; }
        }
        
        /// <summary>
        /// Transforms the polyhedron.
        /// </summary>
        /// <param name="rotation">The rotation matrix.</param>
        /// <param name="translation">The translation vector.</param>
        public void Transform(Matrix rotation, Vector translation)
        {
            if (rotation == null)
            {
                throw new ArgumentNullException("rotation");
            }
        
            if (translation == null)
            {
                throw new ArgumentNullException("translation");
            }
            
            for (int i = 0; i < this.RawVertices.Length; i++)
            {
                this.RawVertices[i].Transform(rotation, translation, this.TransformedVertices[i]);
            }
            
            for (int i = 0; i < this.RawNormals.Length; i++)
            {
                this.RawNormals[i].Transform(rotation, translation, this.TransformedNormals[i]);
            }
        }
    
        /// <summary>
        /// Gets a plane that separates the polyhedron from another polyhedron or null if there is
        /// no separating plane.
        /// </summary>
        /// <param name="other">Another polyhedron.</param>
        /// <returns>The separating plane or null.</returns>
        public Plane GetSeparatingPlane(Polyhedron other)
        {
            if (other == null)
            {
                throw new ArgumentNullException("other");
            }
            
            // Check all the faces of the polyhedron
            foreach (Face face in this.Faces)
            {
                Plane plane = new Plane(face.Vertex1, face.Vertex2, face.Vertex3);
                
                if (this.IsSeparatingPlane(plane, other)) 
                {
                    return plane;
                }
            }
            
            // Check all the faces of the other polyhedron
            foreach (Face face in other.Faces)
            {
                Plane plane = new Plane(face.Vertex1, face.Vertex2, face.Vertex3);
                
                if (other.IsSeparatingPlane(plane, this)) 
                {
                    return plane;
                }
            }
            
            // Check each edge-edge combination
            foreach (Face face in this.Faces) 
            {                
                foreach (Face otherFace in other.Faces) 
                {
                    Plane[] planes = this.BuildEdgeEdgePlanes(face, otherFace);
                    
                    foreach (Plane plane in planes)
                    {
                        if (plane != null && this.IsSeparatingPlane(plane, other))
                        {
                            return plane;    
                        }
                    }
                }
            }
            
            // No separating plane found
            return null;
        }

        /// <summary>
        /// Builds the possible planes from the edge-edge combinations of the two faces.
        /// </summary>
        /// <param name="face1">The first face.</param>
        /// <param name="face2">The second face.</param>
        /// <returns>The planes.</returns>
        private Plane[] BuildEdgeEdgePlanes(Face face1, Face face2)
        {
            Vector[] edges1 = new Vector[] 
            {
                face1.Vertex1 - face1.Vertex2, 
                face1.Vertex2 - face1.Vertex3, 
                face1.Vertex3 - face1.Vertex1 
            };
            
            Vector[] edges2 = new Vector[]
            {
                face2.Vertex1 - face2.Vertex2, 
                face2.Vertex2 - face2.Vertex3, 
                face2.Vertex3 - face2.Vertex1
            };
            
            return new Plane[] 
            {
                this.BuildPlane(face1.Vertex1, edges1[0], edges2[0]),
                this.BuildPlane(face1.Vertex1, edges1[0], edges2[1]),
                this.BuildPlane(face1.Vertex1, edges1[0], edges2[2]),
                this.BuildPlane(face2.Vertex1, edges1[0], edges2[0]),
                this.BuildPlane(face2.Vertex2, edges1[0], edges2[1]),
                this.BuildPlane(face2.Vertex3, edges1[0], edges2[2]),
                this.BuildPlane(face1.Vertex2, edges1[1], edges2[0]),
                this.BuildPlane(face1.Vertex2, edges1[1], edges2[1]),
                this.BuildPlane(face1.Vertex2, edges1[1], edges2[2]),
                this.BuildPlane(face2.Vertex1, edges1[1], edges2[0]),
                this.BuildPlane(face2.Vertex2, edges1[1], edges2[1]),
                this.BuildPlane(face2.Vertex3, edges1[1], edges2[2]),
                this.BuildPlane(face1.Vertex3, edges1[2], edges2[0]),
                this.BuildPlane(face1.Vertex3, edges1[2], edges2[1]),
                this.BuildPlane(face1.Vertex3, edges1[2], edges2[2]),
                this.BuildPlane(face2.Vertex1, edges1[2], edges2[0]),
                this.BuildPlane(face2.Vertex2, edges1[2], edges2[1]),
                this.BuildPlane(face2.Vertex3, edges1[2], edges2[2])
            };
        }
        
        /// <summary>
        /// Builds a plane give a vertex and two edges.
        /// </summary>
        /// <param name="vertex">A vertex on the plane.</param>
        /// <param name="edge1">The first edge direction vector.</param>
        /// <param name="edge2">The second edge direction vector.</param>
        /// <returns>The plane or null if the edges are parallel.</returns>
        private Plane BuildPlane(Vector vertex, Vector edge1, Vector edge2)
        {
            Vector normal = new Vector(edge1).Cross(edge2);
            
            if (normal.IsZero()) 
            {
                return null;
            }
            
            return new Plane(vertex, normal);
        }
        
        /// <summary>
        /// Checks if the given plane separates the polyhedron from another polyhedron.
        /// </summary>
        /// <param name="plane">The plane to check.</param>
        /// <param name="other">The other polyhedron.</param>
        /// <returns>true if the plane separates the polyhedron from the other polyhedron.</returns>
        private bool IsSeparatingPlane(Plane plane, Polyhedron other)
        {
            // Check if all vertices of each polyhedron are on opposite sides of the
            // plane.
            bool above = false;
            bool below = false;
            
            foreach (Vector vertex in other.TransformedVertices) 
            {
                RelativePosition pos = plane.GetRelativePosition(vertex);
                
                above |= pos == RelativePosition.Above;
                below |= pos == RelativePosition.Below;
                
                if (above && below)
                {
                    // Not a valid plane: intersects other polyhedron
                    return false;
                }
            }
            
            // Now check that none of the vertices of this polyhedron are on the same side
            foreach (Vector vertex in this.TransformedVertices) 
            {
                RelativePosition pos = plane.GetRelativePosition(vertex);
                
                if (above && (pos == RelativePosition.Above)) 
                {
                    return false;
                }
                
                if (below && (pos == RelativePosition.Below))
                {
                    return false;
                }
            }
            
            return true;
        }
    }
}
